{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "running_as_notebook = False  # Manually set this to True if you are a human (see #13862)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# PoseSliders\n",
    "\n",
    "Running this cell should allow you to control the 6 DOF pose of a mustard bottle in Meldis."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from IPython.display import display\n",
    "from ipywidgets import ToggleButton\n",
    "import numpy as np\n",
    "\n",
    "from pydrake.common.value import Value\n",
    "from pydrake.geometry import DrakeVisualizer, FramePoseVector, SceneGraph\n",
    "from pydrake.math import RigidTransform\n",
    "from pydrake.multibody.plant import MultibodyPlant\n",
    "from pydrake.multibody.parsing import Parser\n",
    "from pydrake.multibody.tree import BodyIndex\n",
    "from pydrake.systems.analysis import Simulator                                      \n",
    "from pydrake.systems.framework import DiagramBuilder, LeafSystem\n",
    "from pydrake.systems.jupyter_widgets import PoseSliders\n",
    "\n",
    "\n",
    "class PoseToFramePoseVector(LeafSystem):\n",
    "    \"\"\"\n",
    "    Wraps a single pose input into a FramePoseVector.\n",
    "    \"\"\"\n",
    "\n",
    "    def __init__(self, frame_id):\n",
    "        LeafSystem.__init__(self)\n",
    "        self.frame_id = frame_id\n",
    "        self.DeclareAbstractInputPort(\n",
    "          \"pose\", Value(RigidTransform.Identity()))\n",
    "        self.DeclareAbstractOutputPort(\n",
    "          \"vector\", lambda: Value(FramePoseVector()), self.CalcOutput)\n",
    "\n",
    "    def CalcOutput(self, context, output):\n",
    "        pose = self.EvalAbstractInput(context, 0).get_value()\n",
    "        output.get_mutable_value().set_value(id=frame_id, value=pose)\n",
    "      \n",
    "\n",
    "builder = DiagramBuilder()\n",
    "\n",
    "# Note: Don't use AddMultibodyPlantSceneGraph because we are only using\n",
    "# MultibodyPlant for parsing, then wiring our sliders directly to SceneGraph.\n",
    "scene_graph = builder.AddSystem(SceneGraph())\n",
    "plant = MultibodyPlant(time_step=0.0)\n",
    "plant.RegisterAsSourceForSceneGraph(scene_graph)\n",
    "Parser(plant=plant).AddModelsFromUrl(\n",
    "    url=\"package://drake_models/ycb/006_mustard_bottle.sdf\")\n",
    "plant.Finalize()\n",
    "\n",
    "frame_id = plant.GetBodyFrameIdOrThrow(BodyIndex(1))                                            \n",
    "                                            \n",
    "# Add pose sliders.\n",
    "sliders = builder.AddSystem(PoseSliders(\n",
    "    visible=PoseSliders.Visible(),\n",
    "    min_range=PoseSliders.MinRange(), \n",
    "    max_range=PoseSliders.MaxRange(),\n",
    "    value=PoseSliders.Value()))\n",
    "to_vector = builder.AddSystem(PoseToFramePoseVector(frame_id))\n",
    "builder.Connect(sliders.get_output_port(0), to_vector.get_input_port(0))\n",
    "builder.Connect(\n",
    "    to_vector.get_output_port(0),\n",
    "    scene_graph.get_source_pose_port(plant.get_source_id()))\n",
    "\n",
    "# Note: We can't use AddDefaultVisualization because there is no plant\n",
    "# in the Diagram.\n",
    "DrakeVisualizer.AddToBuilder(builder, scene_graph)\n",
    "\n",
    "diagram = builder.Build()\n",
    "simulator = Simulator(diagram)\n",
    "\n",
    "if running_as_notebook:  \n",
    "    simulator.set_target_realtime_rate(1.0)\n",
    "    stop_button = ToggleButton(value=False, description='Stop Simulation')\n",
    "    display(stop_button)\n",
    "    while not stop_button.value:\n",
    "        simulator.AdvanceTo(simulator.get_context().get_time() + 5.0)\n",
    "    stop_button.value = False\n",
    "else:  # running as a test.\n",
    "    simulator.AdvanceTo(0.1)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# WidgetSystem\n",
    "\n",
    "The WidgetsSystem allows you to compose the values of any `ipywidget` widget with `value`s that are convertible to `float`.  Run the first cell to create the system.  Adjust the widgets and run the second cell to confirm it's working."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from ipywidgets import Checkbox, FloatSlider, FloatText, ToggleButtons\n",
    "\n",
    "from pydrake.systems.jupyter_widgets import WidgetSystem\n",
    "\n",
    "checkbox = Checkbox(value=False, description='Check box')\n",
    "slider = FloatSlider(value=3.27, min=0, max=10.0, step=0.1, description='Slider', continuous_update=True)\n",
    "text = FloatText(value=7.5, description='Text')\n",
    "wsg = ToggleButtons(value=0.107, description=\"SchunkWsg\", options=[('Open', 0.107), ('Close', 0.002)])\n",
    "\n",
    "sys = WidgetSystem([checkbox], [slider], [text], [wsg], [checkbox, slider, text, wsg])\n",
    "context = sys.CreateDefaultContext()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for i in range(sys.num_output_ports()):\n",
    "    print(sys.get_output_port(i).Eval(context))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
